/*
 * Copyright The Slam Authors
 */

#include "slam/mapping/slam_evaluator.h"

namespace slam {
namespace mapping {

SlamEvaluator::SlamEvaluator() {}

SlamEvaluator::SlamEvaluator(
    const proto::PoseGraphOptions::SlamEvaluatorOptions& options)
    : options_(options) {}

void SlamEvaluator::AddNode(const NodeId& node_id,
                            const transform::Rigid2d& local_pose) {
  if (has_last_node_ && node_id.trajectory_id == last_node_id_.trajectory_id) {
    const transform::Rigid2d delta_pose =
        last_node_pose_.inverse() * local_pose;
    const double delta_distance = (delta_pose.translation().norm() > 0.03)
                                      ? delta_pose.translation().norm()
                                      : 0.0;
    const double delta_angle = std::fabs(delta_pose.rotation().angle());
    distance_traveled_from_last_connect_ += delta_distance;
    angle_traveled_from_last_connect_ += delta_angle;

    const double distance_traveled_threshold =
        has_global_constraint_ever_
            ? options_.distance_traveled_threshold()
            : options_.distance_traveled_threshold_initial();
    if (distance_traveled_from_last_connect_ > distance_traveled_threshold) {
      LOG(WARNING) << "Localization failed : "
                   << distance_traveled_from_last_connect_ << " "
                   << distance_traveled_threshold;
      is_localization_success_ = false;
    }
  } else {
    has_last_node_ = true;
  }
  last_node_pose_ = local_pose;
  last_node_id_ = node_id;
}

void SlamEvaluator::AddConstraint(
    const std::vector<PoseGraphInterface::Constraint>& constraints) {
  bool has_changed = false;
  LOG(WARNING) << "SlamEvaluator result : "
               << distance_traveled_from_last_connect_ << " ";

  for (const auto& constraint : constraints) {
    if (constraint.submap_id.trajectory_id == 0 &&
        constraint.submap_id.submap_index == -1) {
      if (constraint.node_id.node_index > last_connect_node_id_.node_index ||
          constraint.node_id.trajectory_id !=
              last_connect_node_id_.trajectory_id) {
        last_connect_node_id_ = constraint.node_id;
        has_changed = true;
      } else {
        LOG(WARNING) << "Localization failed : "
                     << distance_traveled_from_last_connect_ << " ";
      }
    }
  }
  if (has_changed) {
    is_localization_success_ = true;
    has_global_constraint_ever_ = true;
    distance_traveled_from_last_connect_ = 0;
    angle_traveled_from_last_connect_ = 0;
  }
}

bool SlamEvaluator::IsLocalizationSuccess() const {
  return is_localization_success_;
}

void SlamEvaluator::Reset() {
  has_last_node_ = false;
  distance_traveled_from_last_connect_ = 0;
  angle_traveled_from_last_connect_ = 0;
  is_localization_success_ = false;
  has_global_constraint_ever_ = false;
}

void SlamEvaluator::ResetTraveledDistance() {
  distance_traveled_from_last_connect_ = 0;
  angle_traveled_from_last_connect_ = 0;
}

}  // namespace mapping
}  // namespace slam
